﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using URWPGSim2D.Core;
using xna = Microsoft.Xna.Framework;
using Microsoft.Xna.Framework;
using URWPGSim2D.Common;
using URWPGSim2D.StrategyLoader;


namespace URWPGSim2D.FishDecision
{
    public static class FishDecision
    {
        #region 弧度转变为度
        public static float RedToDegree(float red)
        {
            float degree = red / (float)Math.PI * 180;
            return degree;
        }
        #endregion

        #region 获得鱼对目标的角度
        public static float GetFishToDestRad(Vector3 cur, Vector3 dest, float fishRad)
        {
            float fishtodestrad = 0;//接收目标对象和鱼的相对角度
            double tan = 0;  //接受正切值
            if (dest.X == cur.X)
            {                                //计算目标对象相当Fish[0]的相对角度bD,值为-PI~PI
                if (dest.Z > cur.Z)
                {
                    fishtodestrad = 90;
                }
                else
                {
                    fishtodestrad = -90;
                }
            }
            else if (dest.X - cur.X > 0)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = RedToDegree((float)Math.Atan(tan));
            }
            else if (dest.Z >= cur.Z)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = 180 + RedToDegree((float)Math.Atan(tan));
            }
            else
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = -180 + RedToDegree((float)Math.Atan(tan));
            }

            float xD = fishtodestrad - RedToDegree(fishRad);          //鱼应该转动的角度，正为顺时，负为逆时
            if (xD < -180)
            {
                xD += 360;
            }
            else if (xD > 180)
            {
                xD -= 360;
            }
            return xD;
        }
        #endregion

        #region 实现鱼转向目标的操作
        public static void FishTrun(Mission mission, ref Decision decision, int teamID, int fishID, Vector3 destinty)
        {
            float fishtodestrad = GetFishToDestRad(mission.TeamsRef[teamID].Fishes[fishID].PositionMm, destinty, mission.TeamsRef[teamID].Fishes[fishID].BodyDirectionRad);



            int tc = 14;            //给予初始的一定角速度排除球和鱼成角180的情况方向初始位顺时针
            int vc = 1;
            if (fishtodestrad > -10 && fishtodestrad < 10)
            {
                vc = 6;
            }
            if (fishtodestrad <= -90)
            {
                tc = 0;
            }
            else if (fishtodestrad < 0)
            {
                tc = 7 + (int)(fishtodestrad / (90 / 4)) - 3;  // 在-90~0度间最大速度为0最小为3
            }
            else if (fishtodestrad < 90)
            {
                tc = 7 + (int)(fishtodestrad / (90 / 4)) + 3;   // 在0~90度间最大速度为14最小为11
            }
            else
            {
                tc = 14;
            }
            decision.TCode = tc;
            decision.VCode = vc;

        }
        #endregion

        #region dribble算法
        /// <summary>
        /// dirbble带球算法 Modified By Zhangbo 2011.10.22
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">临时目标点。</param>
        /// <param name="destDirRad">目标方向。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认6档）。</param>
        /// /// <param name="VCode2">直游档位2（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        public static void Dribble(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm, float destDirRad,
            float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag)
        {
            // 调节所用周期数及每周期毫秒数转换得到秒数
            double seconds1 = 15 * msPerCycle / 1000.0;
            double seconds2 = cycles * msPerCycle / 1000.0;
            // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
            xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
            // 起始点到目标点的距离（目标距离）
            double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
                + Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));

            // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
            double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));
            if (disSrcPtMmToDestPtMm < 0)
            {// 起始点到目标点距离小于阈值（默认58毫米）将中间方向调为目标方向
                dirFishToDestPtRad = destDirRad;
            }

            // 中间方向与鱼体方向的差值（目标角度）
            double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (deltaTheta > Math.PI)
            {// 中间方向为正鱼体方向为负才可能目标角度大于PI
                deltaTheta -= 2 * Math.PI;  // 规范化到(-PI,0)
            }
            else if (deltaTheta < -Math.PI)
            {// 中间方向为负鱼体方向为正才可能目标角度小于-PI
                deltaTheta += 2 * Math.PI;  // 规范化到(0,PI)
            }

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0])
                + Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;

            int code =1;   // 目标（速度）档位初值置1                  //这里改了
            while ((code < 10) && (DataBasedOnExperiment.VCodeAndVelocityTable[code] < targetVelocity))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                code++;
            }
            decision.VCode = code;
            if (Math.Abs(deltaTheta) > angleTheta2 * Math.PI / 180.0)
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }
            else if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (disSrcPtMmToDestPtMm > disThreshold)
                {
                    decision.VCode = VCode1;
                }
                else
                {
                    decision.VCode = VCode2;
                }
            }


            // 以最大速度游过目标距离所需的预计时间（距离预计时间）
            double estimatedTimeByDistance = disSrcPtMmToDestPtMm / DataBasedOnExperiment.VCodeAndVelocityTable[14];
            if (estimatedTimeByDistance > seconds1)
            {// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
                estimatedTimeByDistance = seconds1;
            }
            // 以距离预计时间游过目标角度所需平均角速度（目标角速度）
            double targetAngularV = deltaTheta / estimatedTimeByDistance;

            code = 7;
            if (deltaTheta <= 0)
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((code > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] > targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    code = code - 2;
                }
                if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 12;
                }
                if (code < 0)
                    code = 0;
            }
            else
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((code < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] < targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    code = code + 2;
                }
                if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 2;
                }
                if (code > 14)
                    code = 14;
            }
            decision.TCode = code;
        }
        #endregion

        #region dribble2算法
        /// <summary>
        /// 
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">临时目标点。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的次速度档位。</param>
        /// <param name="angleTheta3">鱼体方向与目标方向角度差的阈值三。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认8档）。</param>
        /// /// <param name="VCode2">直游档位2（默认6档）。</param>
        /// /// <param name="VCode3">直游档位3（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        public static void Dribble2(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm,
            float angleTheta1, float angleTheta2, float angleTheta3, float disThreshold, int VCode1, int VCode2, int VCode3, int cycles, int msPerCycle, bool flag)
        {
            // 调节所用周期数及每周期毫秒数转换得到秒数
            double seconds1 = 15 * msPerCycle / 1000.0;
            double seconds2 = cycles * msPerCycle / 1000.0;
            // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
            xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
            // 起始点到目标点的距离（目标距离）
            double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
                + Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));

            // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
            double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));


            // 中间方向与鱼体方向的差值（目标角度）
            double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (deltaTheta > Math.PI)
            {// 中间方向为正鱼体方向为负才可能目标角度大于PI
                deltaTheta -= 2 * Math.PI;  // 规范化到(-PI,0)
            }
            else if (deltaTheta < -Math.PI)
            {// 中间方向为负鱼体方向为正才可能目标角度小于-PI
                deltaTheta += 2 * Math.PI;  // 规范化到(0,PI)
            }

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0])
                + Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;

            int code = 1;   // 目标（速度）档位初值置1
            while ((code < 10) && (DataBasedOnExperiment.VCodeAndVelocityTable[code] < targetVelocity))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                code++;
            }
            decision.VCode = code;


            if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode1;
                }
                else
                {
                    decision.VCode = VCode1 + 3;
                }
                //decision.VCode = VCode1;
            }
            else if (Math.Abs(deltaTheta) < angleTheta2 * Math.PI / 180.0)
            {
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode2;
                }
                else
                {
                    decision.VCode = VCode2 + 2;
                }
            }
            else if (Math.Abs(deltaTheta) < angleTheta3 * Math.PI / 180.0)
            {
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode2;
                }
                else
                {
                    decision.VCode = VCode2 + 1;
                }
            }
            else
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }

            // 以最大速度游过目标距离所需的预计时间（距离预计时间）
            double estimatedTimeByDistance = disSrcPtMmToDestPtMm / DataBasedOnExperiment.VCodeAndVelocityTable[14];
            if (estimatedTimeByDistance > seconds1)
            {// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
                estimatedTimeByDistance = seconds1;
            }
            // 以距离预计时间游过目标角度所需平均角速度（目标角速度）
            double targetAngularV = deltaTheta / estimatedTimeByDistance;

            code = 7;
            if (deltaTheta <= 0)
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((code > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] > targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    code = code - 1;
                }
                if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 12;
                }
                if (code < 0)
                    code = 0;
            }
            else
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((code < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] < targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    code = code + 1;
                }
                if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 2;
                }
                if (code > 14)
                    code = 14;
            }
            decision.TCode = code;
        }
        #endregion

        #region Dribble2打包
        public static void Dribble3(ref Decision decision, RoboFish fish, xna.Vector3 goal, xna.Vector3 ball)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            float disThreshold = 100;
            int VCode1 = 5;
            int VCode2 = 4;
            int VCode3 = 3;
            int cycles = 10;
            int msPerCycle = 100;
            bool flag = true;
            int degree = (int)GetFishToDestRad(fish.PositionMm, goal, fish.BodyDirectionRad);
            Vector3 setpoint = SetDestPtMm(goal, ball, degree, 40);
            Dribble2(ref decision, fish, setpoint, angleTheta1, angleTheta2, angleTheta3, disThreshold, VCode1, VCode2, VCode3, cycles, msPerCycle, flag);
        }

        #endregion

        #region 获得角度
        /// <summary>
        /// 返回Vector3类型的向量（Y置0，只有X和Z有意义）在场地坐标系中方向的角度值
        /// 场地坐标系定义为：X向右，Z向下，Y置0，负X轴顺时针转回负X轴角度范围为(-PI,PI)的坐标系
        /// </summary>
        /// <param name="v">待计算角度值的xna.Vector3类型向量</param>
        /// <returns>向量v在场地坐标系中方向的角度值</returns>
        public static float GetAngleDegree(xna.Vector3 v)
        {
            float x = v.X;
            float y = v.Z;
            float angle = 0;

            if (Math.Abs(x) < float.Epsilon)
            {// x = 0 直角反正切不存在
                if (Math.Abs(y) < float.Epsilon) { angle = 0.0f; }
                else if (y > 0) { angle = 90.0f; }
                else if (y < 0) { angle = -90.0f; }
            }
            else if (x < 0)
            {// x < 0 (90,180]或(-180,-90)
                if (y >= 0) { angle = (float)(180 * Math.Atan(y / x) / Math.PI) + 180.0f; }
                else { angle = (float)(180 * Math.Atan(y / x) / Math.PI) - 180.0f; }
            }
            else
            {// x > 0 (-90,90)
                angle = (float)(180 * Math.Atan(y / x) / Math.PI);
            }

            return angle;
        }
        #endregion

        #region 获得临时目标点
        /// <summary>
        /// 
        /// </summary>
        /// <param name="goal">为目标坐标</param>
        /// <param name="ball">为目标球的坐标</param>
        /// <param name="setDegree">设置偏转角度</param>
        /// <param name="setDist">为设置的临时目标点相距球的距离 一般30~45</param>
        /// <returns></returns>

        public static Vector3 SetDestPtMm(xna.Vector3 goal, xna.Vector3 ball, int setDegree, int setDist)
        {
            //求 洞指向球 的单位向量
            double xx = (ball.X - goal.X) / Math.Pow(((goal.X - ball.X) * (goal.X - ball.X) + (goal.Z - ball.Z) * (goal.Z - ball.Z)), 0.5);
            double zz = (ball.Z - goal.Z) / Math.Pow(((goal.X - ball.X) * (goal.X - ball.X) + (goal.Z - ball.Z) * (goal.Z - ball.Z)), 0.5);
            float rad = (float)(setDegree * Math.PI / 180);
            double tx = xx * Math.Cos(rad) - zz * Math.Sin(rad);
            double tz = xx * Math.Sin(rad) + zz * Math.Cos(rad);
            Vector3 p = new Vector3();
            p.X = ball.X + (float)tx * setDist;
            p.Z = ball.Z + (float)tz * setDist;
            return p;
        }
        #endregion

        #region 获得目标方向
        public static float GetDirRad(xna.Vector3 cur, xna.Vector3 dest)
        {
            float dirRad = 0;//接收目标对象和鱼的相对角度
            double tan = 0;  //接受正切值
            if (dest.X == cur.X)
            {                                //计算目标对象相当Fish[0]的相对角度bD,值为-PI~PI
                if (dest.Z > cur.Z)
                {
                    dirRad = (float)Math.PI / 2;
                }
                else
                {
                    dirRad = -(float)Math.PI / 2;
                }
            }
            else if (dest.X - cur.X > 0)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = (float)Math.Atan(tan);
            }
            else if (dest.Z >= cur.Z)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = (float)Math.PI + (float)Math.Atan(tan);
            }
            else
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = -(float)Math.PI + (float)Math.Atan(tan);
            }

            return dirRad;

        }
        #endregion

        #region 判断是否在可带球范围
        /// <summary>
        /// 
        /// </summary>
        /// <param name="fish">动作鱼</param>
        /// <param name="ball">目标球</param>
        /// <param name="dest">目标点</param>
        /// <param name="dis">偏置距离，一般是球心后100左右</param>
        /// <param name="radius">范围半径一般为60</param>
        /// <returns></returns>
        public static bool JudgeArea(RoboFish fish, xna.Vector3 ball, xna.Vector3 dest, int setDegree, int dis, int radius)
        {
            float fX = fish.PositionMm.X;
            float fZ = fish.PositionMm.Z;
            Vector3 area = new Vector3();
            area = SetDestPtMm(dest, ball, setDegree, dis);
            float xm = area.X;
            float zm = area.Z;
            if ((fX - xm) * (fX - xm) + (fZ - zm) * (fZ - zm) <= radius * radius)
            {
                return true;
            }
            else
            {
                return false;
            }

        }
        #endregion

        #region 计算鱼所在位置到目标球的角度于目标角度的差
        public static float GetFishAtBallAngleDegree(xna.Vector3 dest, xna.Vector3 ball, xna.Vector3 fish)
        {
            float AngleDegree = 0;
            float rad1 = GetDirRad(dest, ball);
            float rad2 = GetDirRad(fish, ball);
            double Trad = rad2 - rad1;
            if (Trad > Math.PI)
            {
                Trad -= 2 * Math.PI;
            }
            else if (Trad < -Math.PI)
            {
                Trad += 2 * Math.PI;
            }
            AngleDegree = (float)(Trad / Math.PI * 180);
            return AngleDegree;
        }
        #endregion

        #region 计算两点距离
        public static double GetDistBetwGB(xna.Vector3 v1, xna.Vector3 v2)
        {
            return Math.Pow((v1.X - v2.X) * (v1.X - v2.X) + (v1.Z - v2.Z) * (v1.Z - v2.Z), 0.5);
        }
        #endregion

        #region 判断区域
        /// <summary>
        /// 判断目标是否在指定区域内，全场氛围10个区域，整数1-10分别对应相关区域
        /// </summary>
        /// <param name="p">目标坐标</param>
        /// <param name="area">区域编号</param>
        /// <returns>bool</returns>
        public static int JudgePIsInArea(xna.Vector3 p)//边界问题
        {

            if ((p.X <= 2250 && p.X >= 1200) && (p.Z <= 0 && p.Z >= -1070))
            {
                return 0;
            }
            else if ((p.X <= 2250 && p.X >= 1200) && (p.Z >= 0 && p.Z <= 1000))
            {
                return 1;
            }
            else if ((p.X <= 1200 && p.X >= 200) && (p.Z >= 0 && p.Z <= 1000))
            {
                return 2;
            }
            else if ((p.X <= 1000 && p.X >= 200) && (p.Z <= 0 && p.Z >= -850))
            {
                return 3;
            }
            else if ((p.X <= 200 && p.X >= -800) && (p.Z <= 0 && p.Z >= -850))
            {
                return 4;
            }
            else if ((p.X <= 200 && p.X >= -800) && (p.Z <= 1000 && p.Z >= 0))
            {
                return 5;
            }
            else if ((p.X <= -1000 && p.X >= -1600) && (p.Z >= 0 && p.Z <= 1000))
            {
                return 6;
            }
            else if ((p.X <= -1000 && p.X >= -1600) && (p.Z >= -1000 && p.Z <= 0))
            {
                return 7;
            }
            else if ((p.X <= 2250 && p.X >= -2250) && (p.Z >= -1500 && p.Z <= -1070))
            {
                return 10;
            }
            else if ((p.X <= 2250 && p.X >= -2250) && (p.Z >= 1070 && p.Z <= 1500))
            {
                return 12;
            }
            else if ((p.X <= -1600 && p.X >= -2250) && (p.Z >= -1000 && p.Z <= 0))
            {
                return 8;
            }
            else if ((p.X <= -1600 && p.X >= -2250) && (p.Z >= 0 && p.Z <= 1000))
            {
                return 9;
            }
            else
            {
                return 100;
            }


        }
        #endregion

        //#region 判断鱼所在区域
        //public static int FishArea(xna.Vector3 fish)
        //{
        //    int area = 0;
        //    bool fish1 = JudgePIsInArea(fish, 1);
        //    if (fish1 == true)
        //    {
        //        area = 1;
        //    }
        //    bool fish2 = JudgePIsInArea(fish, 2);
        //    if (fish2 == true)
        //    {
        //        area = 2;
        //    }
        //    bool fish3 = JudgePIsInArea(fish, 3);
        //    if (fish3 == true)
        //    {
        //        area = 3;
        //    }
        //    bool fish4 = JudgePIsInArea(fish, 4);
        //    if (fish4 == true)
        //    {
        //        area = 4;
        //    }
        //    bool fish5 = JudgePIsInArea(fish, 5);
        //    if (fish5 == true)
        //    {
        //        area = 5;
        //    }
        //    bool fish6 = JudgePIsInArea(fish, 6);
        //    if (fish6 == true)
        //    {
        //        area = 6;
        //    }
        //    bool fish7 = JudgePIsInArea(fish, 7);
        //    if (fish7 == true)
        //    {
        //        area = 7;
        //    }
        //    bool fish8 = JudgePIsInArea(fish, 8);
        //    if (fish8 == true)
        //    {
        //        area = 8;
        //    }
        //    bool fish9 = JudgePIsInArea(fish, 9);
        //    if (fish9 == true)
        //    {
        //        area = 9;
        //    }
        //    bool fish10 = JudgePIsInArea(fish, 10);
        //    if (fish10 == true)
        //    {
        //        area = 10;
        //    }
        //    return area;
        //}
        //#endregion

        #region 设置定角方向偏置
        /// <summary>
        /// 设置定角方向偏置,获得固定方向，实现鱼的固定方向游动
        /// </summary>
        /// <param name="fish">鱼的坐标</param>
        /// <param name="degree">偏转的角度（+为顺时针，-为逆时针）</param>
        /// <returns>Vector3</returns>
        public static Vector3 SetDegreeFishToP(xna.Vector3 fish, int degree)
        {
            xna.Vector3 p = new Vector3();
            float rad = (float)(degree * Math.PI / 180);
            double tx = 100 * Math.Cos(rad) - 0 * Math.Sin(rad);
            double tz = 100 * Math.Sin(rad) + 0 * Math.Cos(rad);
            p.X = fish.X + (float)tx;
            p.Z = fish.Z + (float)tz;
            return p;
        }
        #endregion

        #region Dribble_Will封装
        /// <summary>
        /// Dribble_Will封装
        /// </summary>
        /// <param name="mission">mission</param>
        /// <param name="decision">ref decision</param>
        /// <param name="teamId">teamId</param>
        /// <param name="fishId">fishId</param>
        /// <param name="ballId">目标球编号</param>
        /// <param name="goal">目标洞坐标</param>
        public static void Dribble_Will(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal,
             float angleTheta1, float angleTheta2, float angleTheta3, int VCode1, int VCode2, int VCode3)
        {
            //接收球的坐标
            xna.Vector3 bM = mission.EnvRef.Balls[ballId].PositionMm;
            //接收鱼的坐标      
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            //防止反带球绕后///////////
            #region 绕后处理
            ////////////如果在鱼驶向目标球后方时，球会被误撞，就给他给他一个偏转角，从侧面过去//////////////////////////////////
            int Tdegree = 0;  //根据情况设置不同的偏置角度
            if (GetFishAtBallAngleDegree(goal, bM, fM) < 80 && GetFishAtBallAngleDegree(goal, bM, fM) > 0)
            {
                Tdegree = -60;
            }
            else if (GetFishAtBallAngleDegree(goal, bM, fM) > -80 && GetFishAtBallAngleDegree(goal, bM, fM) < 0)
            {
                Tdegree = 60;
            }
            #endregion


            //临时目标点//////////////////////////////////处理后置偏转的问题/////////////////////////////////////////////////
            #region 临时目标点
            /////////////////////当球靠近洞口的时候，临时目标点的偏置变量后向后推，经验值51//////////////////////////////////
            xna.Vector3 P = new xna.Vector3();//////存放临时目标点
            int setD = 40;                  ///////偏执变量
                                            //    if ((bM.X > -110 && bM.X < 110) &&
                                            //        (bM.Z < -850 || bM.Z > 850) ||
                                            //        (GetDistBetwGB(bM, goal) <= 550) && goal.X == 0 ||
                                            //        ballId == 7
                                            //        )
                                            //    {
                                            //        setD = 51;
                                            //        //当目标球在左半场且距离洞口很近的时候，偏置放大，防止定过
                                            //        if (bM.X < -250 && goal.X == 0 ||
                                            //            Math.Abs(bM.Z) > 1200)
                                            //        {
                                            //            setD = 61;
                                            //        }
                                            //
                                            //        if (bM.Z > 1330 || bM.Z < -1330)
                                            //        {
                                            //            setD = 10;
                                            //        }
                                            //    }
                                            //    else
                                            //    {
                                            //        setD = 23;
                                            //    }
            #endregion

            //生成临时目标点
            P = SetDestPtMm(goal, bM, 0, setD);


            //判断鱼是否在球后
            bool judge = JudgeArea(mission.TeamsRef[teamId].Fishes[fishId], bM, goal, Tdegree, 250, 250);

            #region Dribble调用

            if (judge)//////判断是否在可顶球的范围
            {
                //         if (goal.X == 0) //中袋
                //         {
                //             if (ballId == 0 && bM.Z < 600)
                //             {
                //
                //                 Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 2, 4, 6, 180, 8, 5, 4, 10, 100, true);
                //             }
                //
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, angleTheta1, angleTheta2, angleTheta3, 200, VCode1, VCode2, VCode3, 10, 100, true);
                //         }
                //
                //         else //底袋
                //         {
                //
                //
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 7, 180, 8, 6, 4, 10, 100, true);
                //             // fish.FishDecision.Dribble(ref decisions[0], mission.TeamsRef[teamId].Fishes[0], P, dirRad, 3, 5, 150, 9, 5, 10, 100, true);
                //         }
                //         //    if (judge2) 
                //         //    {
                //         //        Vector3 tP = fish.FishDecision.SetDestPtMm(goal, mission.EnvRef.Balls[index].PositionMm, 0, 500);
                //         //        fish.FishDecision.Dribble2(ref decisions[0], mission.TeamsRef[teamId].Fishes[0] ,tP, 4, 6, 8, 180, 7, 6, 4, 10, 100, true);
                //         //    }
                //
                //         //中袋洞口处，鱼加速 
                //         if ((bM.Z < -1330 || bM.Z > 1330) && (bM.X > -45 && bM.X < 45))
                //         {
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 9, 300, 13, 10, 4, 10, 100, true);
                //         }
                //
                //        //边带带球加速
                //         else if ((bM.Z < -1150 || bM.Z > 1150) && (bM.X > 2140 || bM.X < -2140) ||
                //             (bM.Z < -1200 || bM.Z > 1200) && (bM.X > 1950 || bM.X < -1950)
                //             )
                //         {
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 9, 300, 13, 10, 4, 10, 100, true);
                //         }
            }
            else  ////距离远时，全速驶向球后
            {

                int Dis = 300;
                //如果球就在洞口则直接冲向球
                if (bM.Z > 1350 || bM.Z < -1350)
                {
                    Dis = 10;
                }



                ///////开球偏置//////////////////////////////////////////////////////////////////////////////
                if (ballId == 0 && mission.TeamsRef[teamId].Fishes[fishId].PositionMm.X < -360)
                {
                    Dis = 600;
                    Tdegree = -65;
                }

                //         fish.FishDecision.Dribble2(
                //              ref decisions[0], mission.TeamsRef[teamId].Fishes[0],
                //              fish.FishDecision.SetDestPtMm(goal, bM, Tdegree, Dis),
                //              6, 11, 30, 1000, 10, 9, 8, 10, 100, true);
                Dribble(
                     ref decision, mission.TeamsRef[teamId].Fishes[fishId],
                     SetDestPtMm(goal, bM, Tdegree, Dis),
                     GetDirRad(mission.TeamsRef[teamId].Fishes[fishId].PositionMm,
                     SetDestPtMm(goal, bM, Tdegree, Dis)) + 3.14f,
                     20, 50, 800, 14, 10, 10, 100, true);

                /////////////////防止尾巴扫球////////////////////////////////////////////////////////
                //         if (bN[index] == 9)
                //         {
                //             if (mission.TeamsRef[teamId].Fishes[0].PositionMm.X > -450)
                //             {
                //                 Vector3 tb = new Vector3(-600, 0, 1000);
                //                 fish.FishDecision.Dribble(
                //                  ref decisions[0], mission.TeamsRef[teamId].Fishes[0],
                //                  fish.FishDecision.SetDestPtMm(goal, tb, Tdegree, 0),
                //                  fish.FishDecision.GetDirRad(mission.TeamsRef[teamId].Fishes[0].PositionMm,
                //                  fish.FishDecision.SetDestPtMm(goal, tb, Tdegree, 0)) + 3.14f,
                //                  10, 40, 800, 9, 4, 10, 100, true);
                //             }
                //         }
                ///===================================================================////
            }
            #endregion


        }
        #endregion

        #region Dribble_Will简化1（确定角度与速度）
        public static void Dribble_Will1(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            int VCode1 = 7;
            int VCode2 = 7;
            int VCode3 = 7;
            Dribble_Will(mission, ref decision, teamId, fishId, ballId, goal, angleTheta1, angleTheta2, angleTheta3, VCode1, VCode2, VCode3);

        }
        #endregion

        #region Dribble_Will简化2（确定角度）
        public static void Dribble_Will2(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal, int VCode1, int VCode2, int VCode3)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            Dribble_Will(mission, ref decision, teamId, fishId, ballId, goal, angleTheta1, angleTheta2, angleTheta3, VCode1, VCode2, VCode3);

        }
        #endregion

        #region 移动到定点并保持姿态
        public static void MoveToAreaS(Mission mission, ref Decision decision, int teamId, int fishId, xna.Vector3 goal, int degree)
        {
            float degreeF = (float)(mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad / Math.PI * 180);
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            if (GetDistBetwGB(fM, goal) < 150)
            {
                xna.Vector3 p = SetDegreeFishToP(fM, degree);
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], p, 60, 150, 180, 200, 13, 13, 3, 10, 100, true);//1,1,3
                if (Math.Abs(degree - degreeF) <= 15)
                {
                    decision.TCode = 7;
                    decision.VCode = 0;
                }
            }
            else if (Math.Abs(degree - degreeF) <= 15 && GetDistBetwGB(fM, goal) < 200)
            {
                decision.TCode = 7;
                decision.VCode = 0;
            }
            else
            {
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], goal, 9, 20, 60, 200, 7, 5, 2, 10, 100, true);
            }

        }
        #endregion

        #region 得到鱼体内一点坐标
        /// <summary>
        /// 得到鱼体内中轴线上一点坐标
        /// </summary>
        /// <param name="mission">mission</param>
        /// <param name="teamId">teamId</param>
        /// <param name="fishId">fishId</param>
        /// <param name="setD">偏置量（从鱼几何中心反向鱼头的距离，‘-’为向鱼头，‘+’相反）</param>
        /// <returns>Vector3</returns>
        public static Vector3 GetFishBodyPM(Mission mission, int teamId, int fishId, int setD)
        {
            Vector3 head = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
            Vector3 bodyIn = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            return SetDestPtMm(head, bodyIn, 0, setD);
        }
        #endregion

        #region 鱼与球之间的距离
        public static float Distance(xna.Vector3 point0, xna.Vector3 point1)
        {
            float distance;
            distance = (float)Math.Sqrt(((point0.X - point1.X) * (point0.X - point1.X)) + ((point0.Z - point1.Z) * (point0.Z - point1.Z)));
            return distance;
        }
        #endregion

        #region 求六个数最小值
        public static float ShortestDistance6(float a, float b, float c, float d, float e, float f)
        {
            float g;
            if (a < b)
            {
                g = a;
            }
            else
            {
                g = b;
            }
            if (g > c)
            {
                g = c;
            }
            if (g > d)
            {
                g = d;
            }
            if (g > e)
            {
                g = e;
            }
            if (g > f)
            {
                g = f;
            }
            return g;
        }
        #endregion

        #region 求十个数的最小值
        public static float ShortestDistance10(float a, float b, float c, float d, float e, float f, float g, float h, float i, float j)
        {
            float k;
            if (a < b)
            {
                k = a;
            }
            else
            {
                k = b;
            }
            if (k > c)
            {
                k = c;
            }
            if (k > d)
            {
                k = d;
            }
            if (k > e)
            {
                k = e;
            }
            if (k > f)
            {
                k = f;
            }
            if (k > g)
            {
                k = g;
            }
            if (k > h)
            {
                k = h;
            }
            if (k > i)
            {
                k = i;
            }
            if (k > j)
            {
                k = j;
            }
            return k;
        }
        #endregion

        #region 输出最近目标
        public static int Target(float a, float b, float c, float d)
        {
            int ball = 0;
            if (a == b)
            {
                ball = 0;
            }
            if (a == c)
            {
                ball = 1;
            }
            if (a == d)
            {
                ball = 2;
            }
            return ball;
        }

        #endregion

        #region 判断左边最近的球是否进过
        public static bool Ifget(int b0_l, int b1_l, int b2_l, int b3_l, int b4_l, int b5_l, int b6_l, int b7_l, int b8_l, int ball)
        {
            if (ball == 0)
            {
                if (b0_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 1)
            {
                if (b1_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 2)
            {
                if (b2_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 3)
            {
                if (b3_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 4)
            {
                if (b4_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 5)
            {
                if (b5_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 6)
            {
                if (b6_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 7)
            {
                if (b7_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 8)
            {
                if (b8_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return false;
            }

        }

        #endregion

        //#region 区域二进球
        //public static void SelfCoutrol2(xna.Vector3 fish, xna.Vector3 ball, Mission mission, ref Decision decision, int teamId, int fishId, RoboFish fish1, int ballId)
        //{
        //    float balltarget = ((-1040 - ball.X) * (fish.Z - ball.Z) / (fish.X - ball.X)) + ball.X;
        //    if ((ball.X - fish.X) * (-1040 - fish.X) >= 0)
        //    {
        //        if (balltarget > -440 && balltarget < 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(-1040, 0, balltarget), ball);
        //        }
        //        else if (balltarget <= -440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(-1040, 0, -440), ball);
        //        }
        //        else if (balltarget >= 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(-1040, 0, 440), ball);
        //        }
        //    }
        //    else
        //    {
        //        if (balltarget >= 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(-1040, 0, ball.Z), ball);
        //        }
        //        else if (balltarget <= -440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(-1040, 0, ball.Z), ball);
        //        }
        //    }
        //}
        //#endregion

        //#region 区域五进球
        //public static void SelfCoutrol5(xna.Vector3 fish, xna.Vector3 ball, Mission mission, ref Decision decision, int teamId, int fishId, RoboFish fish1, int ballId)
        //{
        //    float balltarget = ((1040 - ball.X) * (fish.Z - ball.Z) / (fish.X - ball.X)) + ball.X;
        //    if ((ball.X - fish.X) * (-1040 - fish.X) >= 0)
        //    {
        //        if (balltarget > -440 && balltarget < 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(1040, 0, balltarget), ball);
        //        }
        //        else if (balltarget <= -440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(1040, 0, -440), ball);
        //        }
        //        else if (balltarget >= 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(1040, 0, 440), ball);
        //        }
        //    }
        //    else
        //    {
        //        if (balltarget >= 440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(1040, 0, ball.Z), ball);
        //        }
        //        else if (balltarget <= -440)
        //        {
        //            Dribble3(ref decision, fish1, new Vector3(1040, 0, ball.Z), ball);
        //        }
        //    }
        //}
        //#endregion

        //#region 右半场下半场复位
        //public static void RightRestart(Mission mission, ref Decision decision, int teamId, int fishId, bool judgefish01, bool judgefish02, bool judgefish03,
        //    bool judgefish04, bool judgefish05, bool judgefish06, bool judgefish07, bool judgefish08, bool judgefish09, bool judgefish10, xna.Vector3 fish)
        //{
        //    if (judgefish01 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish02 == true)
        //    {
        //        if (fish.Z < 0)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -700), 0);
        //        }
        //        else
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, 700), 0);
        //        }
        //    }
        //    if (judgefish03 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish04 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish05 == true)
        //    {
        //        if (fish.Z < 0)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, -700), 0);
        //        }
        //        else
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 700), 0);
        //        }
        //    }
        //    if (judgefish06 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish07 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish08 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish09 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //    if (judgefish10 == true)
        //    {
        //        FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //    }
        //}
        //#endregion

        //    #region 左半场下半场复位
        //    public static void LeftRestart(Mission mission, ref Decision decision, int teamId, int fishId, bool judgefish04, bool judgefish05, bool judgefish06,
        //bool judgefish01, bool judgefish02, bool judgefish03, bool judgefish07, bool judgefish08, bool judgefish09, bool judgefish10, xna.Vector3 fish)
        //    {
        //        if (judgefish04 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish05 == true)
        //        {
        //            if (fish.Z < 0)
        //            {
        //                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, -700), 0);
        //            }
        //            else
        //            {
        //                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 700), 0);
        //            }
        //        }
        //        if (judgefish06 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish01 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish02 == true)
        //        {
        //            if (fish.Z < 0)
        //            {
        //                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -700), 0);
        //            }
        //            else
        //            {
        //                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, 700), 0);
        //            }
        //        }
        //        if (judgefish03 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish07 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish08 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish09 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //        if (judgefish10 == true)
        //        {
        //            FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
        //        }
        //    }
        //    #endregion

        //#region 判断进球分数
        //public static int mark(int b0, int b1, int b2, int b3, int b4, int b5, int b6, int b7, int b8)
        //{
        //    int a = 0, b = 0, c = 0, d = 0, e = 0, f = 0, g = 0, h = 0, i = 0, j = 0;
        //    if (b0 == 1)
        //    {
        //        a = 3;
        //    }
        //    if (b1 == 1)
        //    {
        //        b = 3;
        //    }
        //    if (b2 == 1)
        //    {
        //        c = 3;
        //    }
        //    if (b3 == 1)
        //    {
        //        d = 1;
        //    }
        //    if (b4 == 1)
        //    {
        //        e = 1;
        //    }
        //    if (b5 == 1)
        //    {
        //        f = 1;
        //    }
        //    if (b6 == 1)
        //    {
        //        g = 1;
        //    }
        //    if (b7 == 1)
        //    {
        //        h = 2;
        //    }
        //    if (b8 == 1)
        //    {
        //        i = 2;
        //    }
        //    j = a + b + c + d + e + f + g + h + i;
        //    return j;
        //}
        //#endregion

        //#region New2v2SweepBall
        ///// <summary>
        ///// 让鱼在四个角上扫球，让球进入球门范围内，方便进球
        ///// </summary>
        ///// <param name="mission"></param>
        ///// <param name="decision"></param>
        ///// <param name="teamId"></param>
        ///// <param name="fishId"></param>
        ///// <param name="flag">左上-1；左下-3；右上-4；右下-6</param>
        //public static void SweepBall(Mission mission, ref Decision decision, int teamId, int fishId, int flag)
        //{
        //    //一些关键量
        //    Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
        //    Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;

        //    if (flag == 1)
        //    {
        //        if (JudgePIsInArea(fishBody, 2))
        //        {
        //            if (fishHead.X <= -1300f && Math.Abs(fishHead.Z) <= 200)
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //        }

        //        if (JudgePIsInArea(fishBody, 3))
        //        {
        //            Vector3 temp = new Vector3(-660f, 0f, 760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        if (JudgePIsInArea(fishBody, 5) && fishBody.Z >= 0)
        //        {
        //            Vector3 temp = new Vector3(1275f, 0f, 725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 5) && fishBody.Z < 0)
        //        {
        //            Vector3 temp = new Vector3(1275f, 0f, -725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 6))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, 700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 4))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, -700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 10))
        //        {
        //            Vector3 temp = new Vector3(-580f, 0f, 340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 9))
        //        {
        //            Vector3 temp = new Vector3(-580f, 0f, -340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 8))
        //        {
        //            Vector3 temp = new Vector3(-660f, 0f, -760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 7))
        //        {
        //            Vector3 temp = new Vector3(-1080f, 0f, -980f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -150);
        //        }

        //        if (JudgePIsInArea(fishHead, 1))
        //        {
        //            if (fishHead.X <= -1300f && fishHead.Z <= -800f)
        //            {
        //                Vector3 temp = new Vector3(-1425f, 0f, -600f);
        //                FishTrun(mission, ref decision, teamId, fishId, temp);
        //            }
        //            else if (fishHead.X <= -1300f && fishHead.Z > -800f)
        //            {
        //                Vector3 _temp = SetDegreeFishToP(fishBody, +135);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 temp = SetDegreeFishToP(fishBody, -135);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
        //            }
        //        }
        //    }

        //    if (flag == 3)
        //    {
        //        if (JudgePIsInArea(fishBody, 2))
        //        {
        //            if (fishHead.X <= -1300f && Math.Abs(fishHead.Z) <= 200)
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //        }

        //        if (JudgePIsInArea(fishBody, 1))
        //        {
        //            Vector3 temp = new Vector3(-660f, 0f, -760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        if (JudgePIsInArea(fishBody, 5) && fishBody.Z >= 0)
        //        {
        //            Vector3 temp = new Vector3(1275f, 0f, 725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 5) && fishBody.Z < 0)
        //        {
        //            Vector3 temp = new Vector3(1275f, 0f, -725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 6))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, 700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 4))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, -700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 10))
        //        {
        //            Vector3 temp = new Vector3(-1080f, 0f, 980f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 9))
        //        {
        //            Vector3 temp = new Vector3(-660f, 0f, 760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 8))
        //        {
        //            Vector3 temp = new Vector3(-580f, 0f, 340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 7))
        //        {
        //            Vector3 temp = new Vector3(-580f, 0f, -340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -150);
        //        }

        //        if (JudgePIsInArea(fishHead, 3))
        //        {
        //            if (fishHead.X <= -1300f && fishHead.Z >= 800f)
        //            {
        //                Vector3 temp = new Vector3(-1425f, 0f, 600f);
        //                FishTrun(mission, ref decision, teamId, fishId, temp);
        //            }
        //            else if (fishHead.X < -1300f && fishHead.Z < 800f)
        //            {
        //                Vector3 _temp = SetDegreeFishToP(fishBody, -135);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 temp = SetDegreeFishToP(fishBody, +135);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
        //            }
        //        }
        //    }

        //    if (flag == 4)
        //    {
        //        if (JudgePIsInArea(fishBody, 5))
        //        {
        //            if (fishHead.X >= 1300f && Math.Abs(fishHead.Z) <= 200)
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //        }

        //        if (JudgePIsInArea(fishBody, 3))
        //        {
        //            Vector3 temp = new Vector3(660f, 0f, 760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        if (JudgePIsInArea(fishBody, 2) && fishBody.Z >= 0)
        //        {
        //            Vector3 temp = new Vector3(-1275f, 0f, 725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 2) && fishBody.Z < 0)
        //        {
        //            Vector3 temp = new Vector3(-1275f, 0f, -725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 6))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, 700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 1))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, -700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 10))
        //        {
        //            Vector3 temp = new Vector3(580f, 0f, 340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 9))
        //        {
        //            Vector3 temp = new Vector3(580f, 0f, -340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 8))
        //        {
        //            Vector3 temp = new Vector3(660f, 0f, -760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 7))
        //        {
        //            Vector3 temp = new Vector3(1080f, 0f, -980f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -30);
        //        }

        //        if (JudgePIsInArea(fishHead, 4))
        //        {
        //            if (fishHead.X >= 1300f && fishHead.Z <= -800f)
        //            {
        //                Vector3 temp = new Vector3(1425f, 0f, -600f);
        //                FishTrun(mission, ref decision, teamId, fishId, temp);
        //            }
        //            else if (fishHead.X >= 1300f && fishHead.Z > -800f)
        //            {
        //                Vector3 _temp = SetDegreeFishToP(fishBody, +45);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 temp = SetDegreeFishToP(fishBody, -45);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
        //            }
        //        }
        //    }

        //    if (flag == 6)
        //    {
        //        if (JudgePIsInArea(fishBody, 5))
        //        {
        //            if (fishHead.X >= 1300f && Math.Abs(fishHead.Z) <= 200)
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
        //            }
        //        }

        //        if (JudgePIsInArea(fishBody, 3))
        //        {
        //            Vector3 temp = new Vector3(660f, 0f, 760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        if (JudgePIsInArea(fishBody, 2) && fishBody.Z >= 0)
        //        {
        //            Vector3 temp = new Vector3(-1275f, 0f, 725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 2) && fishBody.Z < 0)
        //        {
        //            Vector3 temp = new Vector3(-1275f, 0f, -725f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
        //        }

        //        if (JudgePIsInArea(fishBody, 4))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, -700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 1))
        //        {
        //            Vector3 temp = new Vector3(0f, 0f, -700f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
        //        }

        //        if (JudgePIsInArea(fishBody, 7))
        //        {
        //            Vector3 temp = new Vector3(580f, 0f, -340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 8))
        //        {
        //            Vector3 temp = new Vector3(580f, 0f, 340f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 9))
        //        {
        //            Vector3 temp = new Vector3(660f, 0f, 760f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
        //        }

        //        if (JudgePIsInArea(fishBody, 10))
        //        {
        //            Vector3 temp = new Vector3(1080f, 0f, 980f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, +30);
        //        }

        //        if (JudgePIsInArea(fishHead, 6))
        //        {
        //            if (fishHead.X >= 1300f && fishHead.Z >= 800f)
        //            {
        //                Vector3 temp = new Vector3(1425f, 0f, 600f);
        //                FishTrun(mission, ref decision, teamId, fishId, temp);
        //            }
        //            else if (fishHead.X >= 1300f && fishHead.Z < 800f)
        //            {
        //                Vector3 _temp = SetDegreeFishToP(fishBody, -45);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 temp = SetDegreeFishToP(fishBody, +45);
        //                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
        //            }
        //        }
        //    }
        //}


        //#endregion

        //#region New2v2Disturb
        ///// <summary>
        ///// 捣乱算法
        ///// </summary>
        ///// <param name="mission"></param>
        ///// <param name="decision"></param>
        ///// <param name="teamId"></param>
        ///// <param name="fishId"></param>
        ///// <param name="enemyDoor">敌方球门位置，左侧为0，右侧为1</param>
        //public static void Disturb(Mission mission, ref Decision decision, int teamId, int fishId, int enemyDoor)
        //{
        //    //一些关键量
        //    Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
        //    Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
        //    float fishRad = mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad;

        //    Vector3[] ball = new Vector3[9];
        //    ball[0] = mission.EnvRef.Balls[0].PositionMm;
        //    ball[1] = mission.EnvRef.Balls[1].PositionMm;
        //    ball[2] = mission.EnvRef.Balls[2].PositionMm;
        //    ball[3] = mission.EnvRef.Balls[3].PositionMm;
        //    ball[4] = mission.EnvRef.Balls[4].PositionMm;
        //    ball[5] = mission.EnvRef.Balls[5].PositionMm;
        //    ball[6] = mission.EnvRef.Balls[6].PositionMm;
        //    ball[7] = mission.EnvRef.Balls[7].PositionMm;
        //    ball[8] = mission.EnvRef.Balls[8].PositionMm;

        //    float A = fishBody.Z - fishHead.Z;
        //    float B = fishHead.X - fishBody.X;
        //    float C = fishBody.X * fishHead.Z - fishBody.Z * fishHead.X;


        //    if (enemyDoor == 1)
        //    {
        //        //如果最近的鱼在区域8，快速移动到区域7内
        //        int isFishIn8 = 0;
        //        if (JudgePIsInArea(fishHead, 8))
        //        {
        //            isFishIn8 = 1;
        //        }
        //        if (isFishIn8 == 1)
        //        {
        //            Vector3 temp = new Vector3(770f, 0f, -770f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        //如果最近的鱼在区域9，快速移动到区域10内
        //        int isFishIn9 = 0;
        //        if (JudgePIsInArea(fishHead, 9))
        //        {
        //            isFishIn9 = 1;
        //        }
        //        if (isFishIn9 == 1)
        //        {
        //            Vector3 temp = new Vector3(770f, 0f, 770f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }










        //        //如果最近的鱼在区域7或4，前进到对方球门上方
        //        int isFishIn7Or4 = 0;
        //        if (JudgePIsInArea(fishHead, 7) || JudgePIsInArea(fishHead, 4))
        //        {
        //            isFishIn7Or4 = 1;
        //        }
        //        if (isFishIn7Or4 == 1)
        //        {
        //            Vector3 temp = new Vector3(1200f, 0f, -640f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        //如果最近的鱼在区域10或6，前进到对方球门下方
        //        int isFishIn10Or6 = 0;
        //        if (JudgePIsInArea(fishHead, 10) || JudgePIsInArea(fishHead, 6))
        //        {
        //            isFishIn10Or6 = 1;
        //        }
        //        if (isFishIn10Or6 == 1)
        //        {
        //            Vector3 temp = new Vector3(1200f, 0f, 640f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }










        //        //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(上方)
        //        int isCloseToTargetUp = 0;
        //        Vector3 importantPtUp = new Vector3(1250f, 0f, -600f);
        //        if (Math.Pow(fishBody.X - importantPtUp.X, 2.0) + Math.Pow(fishBody.Z - importantPtUp.Z, 2.0) < Math.Pow(100, 2.0))
        //        {
        //            isCloseToTargetUp = 1;
        //        }
        //        if (isCloseToTargetUp == 1)
        //        {
        //            Vector3 temp = new Vector3(1000f, 0f, -365f);
        //            FishTrun(mission, ref decision, teamId, fishId, temp);
        //        }

        //        //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(下方)
        //        int isCloseToTargetDown = 0;
        //        Vector3 importantPtDown = new Vector3(1250f, 0f, 600f);
        //        if (Math.Pow(fishBody.X - importantPtDown.X, 2.0) + Math.Pow(fishBody.Z - importantPtDown.Z, 2.0) < Math.Pow(100, 2.0))
        //        {
        //            isCloseToTargetDown = 1;
        //        }
        //        if (isCloseToTargetDown == 1)
        //        {
        //            Vector3 temp = new Vector3(1000f, 0f, 365f);
        //            FishTrun(mission, ref decision, teamId, fishId, temp);
        //        }








        //        //如果鱼头进入球门，根据球门内球的分布来决定扫门方向
        //        int ballUp = 0;
        //        if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z < C)
        //            ballUp++;
        //        int ballDown = 0;
        //        if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(fishHead, 5))
        //        {
        //            if (ballUp >= ballDown)
        //            {
        //                Vector3 destiny = SetDegreeFishToP(fishBody, -135);
        //                Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 destiny = SetDegreeFishToP(fishBody, +135);
        //                Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
        //            }
        //        }
        //    }
        //    else
        //    {
        //        //如果最近的鱼在区域8，快速移动到区域7内
        //        int isFishIn8 = 0;
        //        if (JudgePIsInArea(fishHead, 8))
        //        {
        //            isFishIn8 = 1;
        //        }
        //        if (isFishIn8 == 1)
        //        {
        //            Vector3 temp = new Vector3(-770f, 0f, -770f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        //如果最近的鱼在区域9，快速移动到区域10内
        //        int isFishIn9 = 0;
        //        if (JudgePIsInArea(fishHead, 9))
        //        {
        //            isFishIn9 = 1;
        //        }
        //        if (isFishIn9 == 1)
        //        {
        //            Vector3 temp = new Vector3(-770f, 0f, 770f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }










        //        //如果最近的鱼在区域7或1，前进到对方球门上方
        //        int isFishIn7Or1 = 0;
        //        if (JudgePIsInArea(fishHead, 7) || JudgePIsInArea(fishHead, 1))
        //        {
        //            isFishIn7Or1 = 1;
        //        }
        //        if (isFishIn7Or1 == 1)
        //        {
        //            Vector3 temp = new Vector3(-1200f, 0f, -640f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }

        //        //如果最近的鱼在区域10或3，前进到对方球门下方
        //        int isFishIn10Or3 = 0;
        //        if (JudgePIsInArea(fishHead, 10) || JudgePIsInArea(fishHead, 6))
        //        {
        //            isFishIn10Or3 = 1;
        //        }
        //        if (isFishIn10Or3 == 1)
        //        {
        //            Vector3 temp = new Vector3(-1200f, 0f, 640f);
        //            MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
        //        }










        //        //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(上方)
        //        int isCloseToTargetUp = 0;
        //        Vector3 importantPtUp = new Vector3(-1250f, 0f, -600f);
        //        if (Math.Pow(fishBody.X - importantPtUp.X, 2.0) + Math.Pow(fishBody.Z - importantPtUp.Z, 2.0) < Math.Pow(100, 2.0))
        //        {
        //            isCloseToTargetUp = 1;
        //        }
        //        if (isCloseToTargetUp == 1)
        //        {
        //            Vector3 temp = new Vector3(-1000f, 0f, -365f);
        //            FishTrun(mission, ref decision, teamId, fishId, temp);
        //        }

        //        //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(下方)
        //        int isCloseToTargetDown = 0;
        //        Vector3 importantPtDown = new Vector3(-1250f, 0f, 600f);
        //        if (Math.Pow(fishBody.X - importantPtDown.X, 2.0) + Math.Pow(fishBody.Z - importantPtDown.Z, 2.0) < Math.Pow(100, 2.0))
        //        {
        //            isCloseToTargetDown = 1;
        //        }
        //        if (isCloseToTargetDown == 1)
        //        {
        //            Vector3 temp = new Vector3(-1000f, 0f, 365f);
        //            FishTrun(mission, ref decision, teamId, fishId, temp);
        //        }








        //        //如果鱼头进入球门，根据球门内球的分布来决定扫门方向
        //        int ballUp = 0;
        //        if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z < C)
        //            ballUp++;
        //        if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z < C)
        //            ballUp++;
        //        int ballDown = 0;
        //        if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z >= C)
        //            ballDown++;
        //        if (JudgePIsInArea(fishHead, 2))
        //        {
        //            if (ballUp >= ballDown)
        //            {
        //                Vector3 destiny = SetDegreeFishToP(fishBody, +45);
        //                Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
        //            }
        //            else
        //            {
        //                Vector3 destiny = SetDegreeFishToP(fishBody, -45);
        //                Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
        //            }
        //        }
        //    }
        //}

        //#endregion

        //#region !进球!
        ///// <param name="area"> 1-左边 2-右边</param>
        //public static void Goal(int area, Mission mission, ref Decision decision, int teamId, int fishId, int ballId)
        //{
        //    Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
        //    Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
        //    int fisharea = FishArea(fishBody);
        //    Vector3 ball = mission.EnvRef.Balls[ballId].PositionMm;
        //    int ballarea = FishArea(ball);
        //    if (area == 1)
        //    {
        //        if (ballarea == 1)
        //        {
        //            if (fisharea == 1)
        //            {
        //                Dribble_Will1( mission,ref decision,teamId,fishId,ballId, new Vector3(-1350, 0, -450));
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1150, 0, -670), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -540), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), 0);
        //            }
        //        }

        //        if (ballarea == 2)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, -490), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1040, 0, ball.Z), ball);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, 490), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 7 || fisharea == 8)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1050, 0, -950), 0);
        //            }
        //            if (fisharea == 6 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1050, 0, 950), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), 0);
        //            }
        //        }

        //        if (ballarea == 3)
        //        {
        //            if (fisharea == 3)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1350, 0, 450));
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 800), 0);
        //            }
        //        }

        //        if (ballarea == 7)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 7)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1050, 0, -960));
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //        }

        //        if (ballarea == 8)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 8)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1000, 0, -700));
        //            }
        //        }

        //        if (ballarea == 9)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 9)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1050, 0, 700));
        //            }
        //        }

        //        if (ballarea == 10)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 10)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1050, 0, 1000), ball);
        //            }
        //        }

        //        if (ballarea == 4)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1000, 0, -700), ball);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), (int)Math.PI);
        //            }
        //        }

        //        if (ballarea == 5)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, -512), -180);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, 512), -180);
        //            }
        //            if (fisharea == 7 || fisharea == 8)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
        //            }
        //            if (fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1250, 0, -650), ball);
        //            }
        //        }

        //        if (ballarea == 6)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1000, 0, 700), ball);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), (int)Math.PI);
        //            }
        //        }

        //    }

        //    else if (area == 2)
        //    {
        //        if (ballarea == 1)
        //        {
        //            if (fisharea == 1)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-700, 0, -900), ball);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, -950), -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), -180);
        //            }
        //        }

        //        if (ballarea == 2)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, -512), -180);
        //            }
        //            if (fisharea == 2)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1400, 0, 650), ball);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, 512), -180);
        //            }
        //            if (fisharea == 4 || fisharea == 7 || fisharea == 8)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, -950), -180);
        //            }
        //            if (fisharea == 6 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), -180);
        //            }
        //        }

        //        if (ballarea == 3)
        //        {
        //            if (fisharea == 3)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-700, 0, 900), ball);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 800), -180);
        //            }
        //        }

        //        if (ballarea == 7)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 7)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, -960));
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //        }

        //        if (ballarea == 8)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 8)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, -700));
        //            }
        //        }

        //        if (ballarea == 9)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 9)
        //            {
        //                Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, 700));
        //            }
        //        }

        //        if (ballarea == 10)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
        //            }
        //            if (fisharea == 10)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1024, 0, 1000), ball);
        //            }
        //        }

        //      /*  if (ballarea == 4)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                SweepBall(mission, ref decision, teamId, fishId, 4);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1150, 0, -670), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 450), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
        //            }
        //        }

        //        if (ballarea == 5)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, -512), -180);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, 512), -180);
        //            }
        //            if (fisharea == 7 || fisharea == 8)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
        //            }
        //            if (fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
        //            }
        //            if (fisharea == 5)
        //            {
        //                SelfCoutrol5(fishBody, ball, mission, ref decision, teamId, fishId, mission.TeamsRef[teamId].Fishes[fishId], ballId);
        //            }
        //        }

        //        if (ballarea == 6)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1250, 0, 512), ball);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
        //            }
        //        }*/

        //    }
        //}
        //#endregion /

        //#region !志浩函数!
        ///// <param name="area"> 1-左边 2-右边</param>
        //public static void ZhiHaoJump(int area,Mission mission,ref Decision decision,int fishId,int teamId)
        //{
        //    Vector3 MyFish = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
        //    Vector3 MyFishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
        //    int fisharea = FishArea(MyFishHead);
        //    if (area==1)
        //    {
        //        if (fishId == 1)
        //        {
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, -555), (int)Math.PI);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, -565), (int)Math.PI);
        //            }
        //            if(fisharea==3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), (int)Math.PI);
        //            }
        //            if(fisharea==4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), (int)Math.PI);
        //            }
        //            if(fisharea==5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1350, 0, -750), (int)Math.PI);
        //            }
        //            if(fisharea==6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), (int)Math.PI);
        //            }
        //            if(fisharea==7 || fisharea==8 || fisharea==9 || fisharea==10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1260,0,-720), (int)Math.PI);
        //            }
        //        }
        //        if (fishId == 0)
        //        {
        //            if (fisharea == 1)
        //            {
        //                 MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), (int)Math.PI);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, 565), (int)Math.PI);
        //            }
        //            if (fisharea == 3)
        //            {
        //               MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, 555), (int)Math.PI);
        //            }
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), (int)Math.PI);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1350, 0, 750), (int)Math.PI);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), (int)Math.PI);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1260, 0, 720), (int)Math.PI);
        //            }
        //        }
        //    }

        //    else if (area == 2)
        //    {
        //        if (fishId == 1)
        //        {
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, -555), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, -565), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
        //            }
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1350, 0, -750), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1260, 0, -720),0);
        //            }
        //        }
        //        if (fishId == 0)
        //        {
        //            if (fisharea == 4)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
        //            }
        //            if (fisharea == 5)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, 565), 0);
        //            }
        //            if (fisharea == 6)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, 555), 0);
        //            }
        //            if (fisharea == 1)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
        //            }
        //            if (fisharea == 2)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1350, 0, 750), 0);
        //            }
        //            if (fisharea == 3)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
        //            }
        //            if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
        //            {
        //                MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1260, 0, 720), 0);
        //            }
        //        }
        //    }
        //}
        //#endregion

        #region 判断球进洞
        public static int  Panduanballjindong (xna.Vector3 p)
        {
            if((p.X>=1800&&p.X<=2000)&&(p.Z>=-1300&&p.Z<=-1100))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }
        #endregion

        #region 移动到定点并保持姿态
        public static void MoveToAreaS_2(Mission mission, ref Decision decision, int teamId, int fishId, xna.Vector3 goal, int degree)
        {
            float degreeF = (float)(mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad / Math.PI * 180);
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            if (GetDistBetwGB(fM, goal) < 150)
            {
                xna.Vector3 p = SetDegreeFishToP(fM, degree);
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], p, 60, 150, 180, 200, 14, 14, 3, 10, 100, true);//1,1,3
                if (Math.Abs(degree - degreeF) <= 15)
                {
                    decision.TCode = 7;
                    decision.VCode = 0;
                }
            }
            else if (Math.Abs(degree - degreeF) <= 15 && GetDistBetwGB(fM, goal) < 200)
            {
                decision.TCode = 7;
                decision.VCode = 0;
            }
            else
            {
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], goal, 9, 20, 60, 200, 7, 5, 2, 10, 100, true);
            }

        }
        #endregion
    }
}